#include "Constraint.h"
#include "MultibodySystem.h"
#include "FastOps.h"
#include "Quaternion.h"

using namespace RSIM;

/////////////////////////////////////////////////////// 

Constraint::~Constraint(){
        if(Child_cBetak_)
                delete [] Child_cBetak_;
        if(Parent_cBetak_)
                delete [] Parent_cBetak_;        
}

/////////////////////////////////////////////////////// 

int Constraint::getParentSysId()const{
        return pMBS_->getSysId();
}

/////////////////////////////////////////////////////// 

int Constraint::getChildSysId()const{
        if(cMBS_)
                return cMBS_->getSysId();
        else
                return -1;
}
                
/////////////////////////////////////////////////////// 

void Constraint::allocateSpaceForBetak(){        
        Parent_cBetak_ = new Vect3[Parent_Path_.getLength()];
        
        // If child mbs is ground, no additional space is needed.
        const int& clen = Child_Path_.getLength();
        if(clen){
                Child_cBetak_ = new Vect3[clen];
        }
}

///////////////////////////////////////////////////////

void Constraint::calcCumulativeBeta(){
        // For parent branch
        calcCumulativeBetaForParentBranch();                
        
        // For child branch
        calcCumulativeBetaForChildBranch();
}

///////////////////////////////////////////////////////

void Constraint::init(){                
        
        // Set size of error vectors
        Pos_Error_.resize(getNAlgebraicConstraints());
        Vel_Error_.resize(getNAlgebraicConstraints());
}

///////////////////////////////////////////////////////

void Constraint::initConstraintBodyFixedTransforms(){
                        
        const RigidBodyNode *pnode = pMBS_->getRigidBodyNode(ParentNodeId_);
        
        const Vect3& p_rk = pnode->data_->getBodyRK();
        const Transform& pT_BF = pnode->data_->getTransformBF();
        
        Parent_Bod_FM_.wgetP() = p_rk + (~pT_BF.R())*T_PM_.P();
        Parent_Bod_FM_.wgetR() = (~pT_BF.R())*T_PM_.R();
        
        if(cMBS_){
                // if child systems is not ground                
                const RigidBodyNode *cnode = cMBS_->getRigidBodyNode(ChildNodeId_);
                const Vect3& c_rk = cnode->data_->getBodyRK();
                const Transform& cT_BF = cnode->data_->getTransformBF();
                
                Child_Bod_FM_.wgetP() = c_rk + (~cT_BF.R())*T_BF_.P();
                Child_Bod_FM_.wgetR() = (~cT_BF.R())*T_BF_.R();
        }
        else{
                // This is the case when child is ground
                Child_Bod_FM_.wgetP() = T_BF_.P();
                Child_Bod_FM_.wgetR() = T_BF_.R();
        }
}

/////////////////////////////////////////////////////// 

void Constraint::calcCumulativeBetaForParentBranch(){
                
        const int& plen = Parent_Path_.getLength();        
        
        // Parent cannot be ground, so this is fine.
        const RigidBodyNode *Nd = pMBS_->getRigidBodyNode(Parent_Path_[0]);
        const Rotation& NF = Nd->data_->getRotationNF();
        
        // Parent_cbetak[0]= NF*Parent_FM.P();
        FastOps::mult_mat33_vec3(NF.Ptr(),
                                 Parent_Bod_FM_.P().Ptr(),
                                 Parent_cBetak_[0].wPtr()
                                 );

        for(int k=1;k<plen;++k){                                                                
                const RigidBodyNode *Nd = pMBS_->getRigidBodyNode(Parent_Path_[k-1]);
                Parent_cBetak_[k] = Parent_cBetak_[k-1] + Nd->data_->getnbetak(); // sFlag
        }                
}

/////////////////////////////////////////////////////// 

void Constraint::calcCumulativeBetaForChildBranch(){
        
        // If the child multibody system is not ground 
        if(cMBS_){
                // if child multibody system is not ground, clen is certainly non-zero
                const int& clen = Child_Path_.getLength();
                
                const RigidBodyNode *Nd = cMBS_->getRigidBodyNode(Child_Path_[0]);                                
                const Rotation& NF = Nd->data_->getRotationNF();                                
                
                // Parent_cbetak[0]= NF*Parent_FM.P();
                FastOps::mult_mat33_vec3(NF.Ptr(),
                                         Child_Bod_FM_.P().Ptr(),
                                         Child_cBetak_[0].wPtr()
                                        );                                        

                for(int k=1;k<clen;++k){
                        const RigidBodyNode *Nd = cMBS_->getRigidBodyNode(Child_Path_[k-1]);
                        Child_cBetak_[k] = Child_cBetak_[k-1] + Nd->data_->getnbetak(); // sFlag
                }
        }
        
        // Nothing to do if child multibody system is ground since betak is then directly obtained 
        // from  Child_Bod_FM_ ! 
}

/////////////////////////////////////////////////////// 

void Constraint::updQ(const Vector& delq, int& qid){
        const double *dq = delq.Ptr();
        
        // update the q's associated with the Parent branch
        ConstraintUtils::updBranchQs(dq,
                                     pumap_,
                                     Parent_Path_,
                                     pMBS_,
                                     pstate_,
                                     qid);
        
        // update the q's associated with the child branch        
        ConstraintUtils::updBranchQs(   dq,
                                        cumap_,
                                        Child_Path_,
                                        cMBS_,
                                        cstate_,
                                        qid);                
}

/////////////////////////////////////////////////////// 